#include "Planning.h"
#include <windows.h>
#include <iostream>
#include <fstream>
#include <sstream>
#include <stdio.h>
#include <time.h>

Planning::Planning(void)
{
	//file = ofstream("output.txt",ios::out, ios::app);
}

Planning::~Planning(void)
{
	file.close();
}

void Planning::Solve(ostream &osw, bool showBoard, string header, Search &search, vector<string> maps, vector<string> paths)
{
	try {
		osw.write("=======================================================================\n", 72);
		osw.write(header.c_str(), header.length()); osw.write("\n", 1);
		osw.write("=======================================================================\n", 72);
		osw.write(" N  ?  Len  Cost       Nodes     MSec  Solution\n", 48);
		for (int n=0; n < (int) maps.size(); ++n) {

			// Load in the map.
			if (!gws.loadMap(maps.at(n))) {
				printf("Error: loading file: %s\n", maps.at(n).c_str());
				return;
			}

			// Load in the paths.
			vector<PathingRequest> pathingRequests;
			if (! loadPaths(paths.at(n), pathingRequests))
			{
				printf("Error: loading file: %s\n", paths.at(n).c_str());
				return;
			}

			if (showBoard) {
				gws.draw(NULL);
				cout.flush();
			}

			// Solve the problem instances
			for (int i=0; i < (int) pathingRequests.size(); ++i)
			{
				if (!gws.setAgent(pathingRequests.at(i).start) || 
					!gws.setGoal(pathingRequests.at(i).goal)   ||
					!gws.setStart(pathingRequests.at(i).start)
				 )
				{
					printf("Error: could not set start/goal for path number %d\n", i);
					continue;
				}
				Solution sol;
				Cost cost = 0;
				
				//QueryPerformanceFrequency((LARGE_INTEGER*) &tickFrequency);
				//QueryPerformanceCounter((LARGE_INTEGER*) &tickCountBegin);
				clock_t start = clock();
				bool solved = search.run(gws, cost, sol);
				clock_t end = clock();
				//QueryPerformanceCounter((LARGE_INTEGER*) &tickCountEnd);
				//double tim = (tickCountEnd - tickCountBegin) / (double) tickFrequency;
				double tim = end - start;

				// Draw map, showing solution.
				if (showBoard) {
					gws.draw(&sol);
					cout.flush();
				}

				// Output search logistic (to console and to a file).
				OutputInfo(cout, n, i, solved, (long) search.getNodesExpanded(), sol, tim, &gws);
				OutputInfo(osw, n, i, solved, (long) search.getNodesExpanded(), sol, tim, &gws);
				printf("\n");
			}
		}
	} catch (iostream::failure e) {
		cout << e.what() << endl;
	}

}

void Planning::Solve(ostream &osw, bool showBoard, string header, Search &search){
	try {
		osw.write("=======================================================================\n", 72);
		osw.write(header.c_str(), header.length()); osw.write("\n", 1);
		osw.write("=======================================================================\n", 72);
		osw.write(" N  ?  Len  Cost       Nodes     MSec  Solution\n", 48);
		if (showBoard) {
			gws.draw(NULL);
			cout.flush();
		}
		sol.clear();
		// Solve the problem instances

		gws.setAgent(gws.getStart());
		Cost cost = 0;
			
		//QueryPerformanceFrequency((LARGE_INTEGER*) &tickFrequency);
		//QueryPerformanceCounter((LARGE_INTEGER*) &tickCountBegin);
		clock_t start = clock();
		bool solved = search.run(gws, cost, sol);
		clock_t end = clock();
		//QueryPerformanceCounter((LARGE_INTEGER*) &tickCountEnd);
		//double tim = (tickCountEnd - tickCountBegin) / (double) tickFrequency;
		double tim = end - start;

		// Draw map, showing solution.
		//*
		if (showBoard) {
			gws.draw(&sol);
			cout.flush();
		}//*/
		// Output search logistic (to console and to a file).
		OutputInfo(cout, 0, 0, solved, (long) search.getNodesExpanded(), sol, tim, &gws);
		// OutputInfo(osw, 0, 0, solved, (long) search.getNodesExpanded(), sol, tim, &gws);
		printf("\n");
	} catch (iostream::failure e) {
		cout << e.what() << endl;
	}
}

void Planning::OutputInfo(ostream &osw, int n, int pathid, bool solved, 
						  long nodesExpanded, Solution sol, double tim, GridworldState* gws)
{
	try {
		char buf[80];
		int i=0;
		
		_itoa_s(n, buf, 10);
		osw.write(buf, i=strlen(buf));
		osw.write(" ", 1);
		
		_itoa_s(pathid, buf, 10);
		osw.write(buf, i=strlen(buf));
		osw.write(" ", 1);
		
		osw.write(solved ? "1" : "0", 1);
		osw.write(" ", 1);
		
		_itoa_s(sol.size(), buf, 10);
		osw.write(buf, strlen(buf));
		osw.write(" ", 1);
		
		//itoa(sol.m_cost, buf, 10);
		osw.write(buf, strlen(buf));
		osw.write(" ", 1);
		
		_ltoa_s(nodesExpanded, buf, 10);
		osw.write(buf, i=strlen(buf));
		osw.write(" ", 1);
		
		sprintf_s(buf, "%f", tim);
		osw.write(buf, i=strlen(buf));
		osw.write(" ", 1);

		for (int i=0; i < (int) sol.size(); ++i)
		{
			GWAction a = sol.at(i);
			string str = gws->getGWActionName(a);
			osw.write(str.c_str(), str.length());
			osw.write(" ", 1);
		}

		osw.write("\n", 1);
		osw.flush();
	} catch (iostream::failure e) {
		cout << e.what() << endl;
	}

}

bool Planning::loadPaths(string fileName, vector<PathingRequest> &pathData)
{
	try {
		char line[2000];
		fstream fin(fileName.c_str(), ios::in);

		while(!fin.eof()) {
			fin.getline(line,2000);
			vector<string> st = tokenize(line, " \t\n\r\f");
			string start = "";
			string goal = "";
			int idx = 0;
			if (idx < (int) st.size()) {			// hasMoreTokens
				string no = st.at(idx++);			// request number, ignored.
				if (idx < (int) st.size()) {
					start = st.at(idx++);
				}
				if (idx < (int) st.size()) {
					goal = st.at(idx++);
				}
				if (!(start.empty() && goal.empty())) {
					PathingRequest pr;
					pr.start = atoi(start.c_str());
					pr.goal = atoi(goal.c_str());
					pathData.push_back(pr);
				}
				else {
					if (strlen(line) > 0)
						printf("Error: Reading in path requests.\n");
				}
			}
		}
		fin.close();

	} catch (iostream::failure e) {
		cout << e.what() << endl;
		cout << "Error: Unable to find/read file: \n";
		cout << fileName << endl;
		return false;
	}

	return true;
}


void Planning::Test(Mapping* map) {
	bool showBoard = true; // Show game board.
	SearchAStar sAStar;		// or Search &sAStar = *(new SearchAStar);
	
	if ( map == 0) {
	// Maps to use for testing.
		vector<string> maps;
		vector<string> paths;
		maps.push_back("maze03.map");
		paths.push_back("maze03.paths");
	
	/*
		If you prefer DFS than AStar
		SearchDFID  sDFID;
		Search *psDFID = new SearchDFID;
		Solve(file, showBoard, "DFID", sDFID, maps, paths);
	*/
	
		try {
			ofstream file("output.txt"); // Store output in file.
			Solve(file, showBoard, "ASTAR", sAStar, maps, paths);  // Replace above statement with this one when ready.	
			file.close();
		}
		catch (iostream::failure e) {
			cout << e.what() << endl;
		}
	}
	else {
		gws.loadMapFromPlanning(map->getBooleanMap(), map->getRows(), map->getCols());
		Location a = map->getAgentLocation();
		int agentLinearPos =  LocationToLinarPos( a );
		gws.setStart(0);
		if (agentLinearPos >0 && agentLinearPos < gws.getSize()) {
			gws.setStart(agentLinearPos);
		}
		if (map->getTargetLocationsCounter() > 1) {
			int nextTarget = 1;
			Location targetLocation = map->getTargetLocation(nextTarget);
			int targetLinearPos = LocationToLinarPos(targetLocation);
			gws.setGoal(gws.getSize() -1);
			if (targetLinearPos > 0 && targetLinearPos < gws.getSize() ) {
				gws.setGoal(targetLinearPos);
			}
		}
		try {
			ofstream file("output.txt"); // Store output in file.
			Solve(file, showBoard, "ASTAR", sAStar);  // Replace above statement with this one when ready.	
			file.close();
		}
		catch (iostream::failure e) {
			cout << e.what() << endl;
		}
	}
}

Solution* Planning::getPath( Mapping* map, int target ) {
	bool showBoard = false; // Show game board.
	SearchAStar sAStar;		// or Search &sAStar = *(new SearchAStar);
	gws.loadMapFromPlanning(map->getBooleanMap(), map->getRows(), map->getCols());
		Location a = map->getAgentLocation();
		int agentLinearPos =  LocationToLinarPos( a );
		gws.setStart(0);
		if (agentLinearPos >0 && agentLinearPos < gws.getSize()) {
			gws.setStart(agentLinearPos);
		}
		if (map->getTargetLocationsCounter() > -1) {
			int nextTarget = target;
			Location targetLocation = map->getTargetLocation(nextTarget);
			int targetLinearPos = LocationToLinarPos(targetLocation);
			gws.setGoal(gws.getSize() -1);
			if (targetLinearPos > 0 && targetLinearPos < gws.getSize() ) {
				gws.setGoal(targetLinearPos);
			}
		}
		try {
			Solve(file, showBoard, "ASTAR", sAStar);
		}
		catch (iostream::failure e) {
			cout << e.what() << endl;
		}
	return &sol;
}

vector<string> Planning::tokenize(const string &str, const string &delim)
	{
	vector<string> tokens;
	size_t p0 = 0, p1 = string::npos;
	while(p0 != string::npos)
	{
		p1 = str.find_first_of(delim, p0);
		if(p1 != p0)
		{
			string token = str.substr(p0, p1 - p0);
			tokens.push_back(token);
		}
		p0 = str.find_first_not_of(delim, p1);
	}
	return tokens;
}

int Planning::LocationToLinarPos ( Location inn ) {
	
	int ret = (int) (inn.getY()) * (gws.getRowCount()-1);
	ret +=  (int)inn.getY();
	ret +=  (int)inn.getX();
	
	return ret;
}
